2024.7.8 rk4法によるばね-マス-ダンパ系のPID制御【numpy】

PID制御は「前ステップ」の状態を保持し、偏差の微分、積分値を近似する必要がある。

しかし、RK4法は４回の関数呼び出しを行うので、これをそのまま

code:bad_sample.py

xd = Func_xd(params, h)

（略）

k1 = xd(t, x)

k2 = xd(t + h*0.5, x + k1*h*0.5)

k3 = xd(t + h*0.5, x + k2*h*0.5)

k4 = xd(t + h, x + k3*h)

と書いてしまうと、各段の関数処理内においては「前ステップ」ではなく「前段の関数呼び出し」における状態が用いられることになるため、正しく計算を行うことができない。

そこで、各段で呼び出す関数を個別のインスタンスとすることで、各段の呼び出し時に前ステップの状態を利用するようにした。

code:corrected_sample.py

xd1 = Func_xd(params, h)

xd2 = Func_xd(params, h)

xd3 = Func_xd(params, h)

xd4 = Func_xd(params, h)

（略）

k1 = xd1(t, x)

k2 = xd2(t + h*0.5, x + k1*h*0.5)

k3 = xd3(t + h*0.5, x + k2*h*0.5)

k4 = xd4(t + h, x + k3*h)

このやり方が正しいという情報を見つけることはできていないが、オイラー法の結果と比較を通じて正しいという強い確信を得ている。

code:pid.py

import numpy as np

import matplotlib.pyplot as plt

class Func_xd:

def __init__(self, params, h):

A, B, C, Kp, Ki, Kd, r = params

self.h = h # 時間の刻み幅

self.r = r # 目標値

self.A, self.B, self.C = A, B, C

self.Kp, self.Ki, self.Kd = Kp, Ki, Kd

self.e_int = 0 # 偏差の積分値の初期値

self.e_pre = 0 # 偏差の初期値

def __call__(self, _, x):

return self.xd(x)

def xd(self, x):

y = self.C@x

e_now = self.r - y

self.e_int = self.e_int + e_now*self.h

e_dif = (e_now - self.e_pre)/self.h

u = self.Kp@e_now + self.Ki*self.e_int + self.Kd*e_dif

xd = self.A@x + self.B@u

self.e_pre = e_now

return xd

class Solver_ODE:

def __init__(self, x0, t_span, h, params):

self.x0 = x0

self.t_span = t_span

self.h = h

self.params = params

def solve_euler(self):

hist_x, hist_t = [], []

h = self.h

x = np.array(self.x0, dtype=float).reshape(dim_m, 1)

xd = Func_xd(params, h)

hist_t.append(t)

hist_x.append(x.flatten())

x = x + h*xd(t, x)

t = t + h

self.hist_t = np.stack(hist_t)

self.hist_x = np.stack(hist_x)

def solve_rk4(self):

hist_x, hist_t = [], []

x = np.array(self.x0, dtype=float).reshape(dim_m, 1)

h = self.h

xd1 = Func_xd(params, h)

xd2 = Func_xd(params, h)

xd3 = Func_xd(params, h)

xd4 = Func_xd(params, h)

hist_t.append(t)

hist_x.append(x.flatten())

k1 = xd1(t, x)

k2 = xd2(t + h*0.5, x + k1*h*0.5)

k3 = xd3(t + h*0.5, x + k2*h*0.5)

k4 = xd4(t + h, x + k3*h)

x = x + h*(k1 + 2.0*k2 + 2.0*k3 + k4)/6.0

t = t + h

self.hist_t = np.stack(hist_t)

self.hist_x = np.stack(hist_x)

##

h_eul = 0.0001 # 時間の刻み幅 for Euler method

h_rk4 = 0.01 # 時間の刻み幅 for RK4 method

m, c, k = 1.0, 1.0, 10.0 # 質量、減衰係数、ばね定数

r = 1.0 # 目標

Kp = 1.0 # Pゲイン、適当に設定

Ki = 10.0 # Iゲイン、適当に設定

Kd = 10.0 # Dゲイン、適当に設定

##

dim_m, dim_n = B.shape # n-状態, m-入力

Kp = np.array(Kp).reshape(1, 1)

Ki = np.array(Ki).reshape(1, 1)

Kd = np.array(Kd).reshape(1, 1)

params = (A, B, C, Kp, Ki, Kd, r)

sol_eul = Solver_ODE(x0, t_span, h_eul, params)

sol_eul.solve_euler()

sol_rk4 = Solver_ODE(x0, t_span, h_rk4, params)

sol_rk4.solve_rk4()

plt.subplot(2, 1, 1)

plt.title('$y$')

plt.grid()

plt.legend()

plt.subplot(2, 1, 2)

plt.title('$\dot{y}$')

plt.grid()

plt.legend()

plt.subplots_adjust(wspace=0.2, hspace=0.4) # 余白設定

plt.show()

https://scrapbox.io/files/668b49b14f0539001da00cb1.png

時間の刻み幅を２手法で個別に設定できるようにした。